//Elevator Controller
 
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <signal.h>
 
#include <native/task.h>
#include <native/mutex.h>
#include <native/cond.h>
#include <native/sem.h>

#include <time.h>
 
#include <sys/mman.h>
#include <netdb.h>
#include <unistd.h>

//This value should be the desired amount of time between floors,
//in milliseconds, divided by the time between updates (75 ms)
//Before Milestone 4, it was about 3 seconds (value of 40).
//With milestone 4, we want 2 seconds.  Value of 27 is 2025 milliseconds,
//which is  as close as we can get.
#define TICKS_BETWEEN_FLOORS 27 

#define UP 1
#define DOWN 0
#define IDLE 2
#define BOTH 3
 
//75 ms between status updates
#define TIME_BETWEEN_UPDATES 75
//Elevator message is 255 bytes at most
#define MAX_MESSAGE_SIZE 255 
 
//As defined in the project description
#define MAX_NUM_FLOORS 30
 
#define BASE_UDP_PORT 10000
//Highest port is arbitrary in this case
#define MAX_UDP_PORT 15000
 
//Length of port string is 5 digits, plus null
#define PORT_STRING_LENGTH 6
 
//When there is no data in the double buffer, we need to wait
#define NO_DATA_BUFFED 2
 
//The default amount of time the elevator door stays open, in seconds
//For milestone 4, we set this to 5.  Was 15
#define DEFAULT_DOOR_TIME 5
 
//Time between supervisor thread iterations, in milliseconds
#define SUPERVISOR_THREAD_PERIOD 500
 
//1 Millisecond in nanoseconds
#define MILLISECONDS 1000000
 
//Maximum lengths for hostname and port
#define MAX_HOSTNAME_LENGTH 50
#define MAX_PORT_LENGTH     10
 
//Elevator capacity, in pounds
#define MAX_ELEVATOR_CAPACITY 2000
 
///////////////////////////////////////////////////////
 
//Elevator Message Types:
#define ELEVATORREGISTRATION      0x0F
#define ELEVATORREGISTRATIONREPLY 0x0E
#define ELEVATORDEREGISTRATION    0x7F
#define ELEVATORSTATUS            0x01
#define ELEVATORPING              0x02
#define ELEVATORDESTINATION       0x05
#define ELEVATORDESTINATIONREMOVE 0x06
#define KILLORDER                 0x66
 
 
///////////////////////////////////////////////////////
 
//PersonDispatcher Message Types:
#define PERSONTRANSFER       0x11
#define PERSONINELEVATOR     0x12
#define PERSONPRESSBUTTON    0x13
#define PERSONOUTELEVATOR    0x14
 
#define GENERALACK           0x55
 
#define SUCCESS              1
#define FAIL                 0
 
#define IN                   1
#define OUT                  0
 
///////////////////////////////////////////////////////
//Elevator Buttons
 
#define HOLDDOOR 0x64
#define CLOSEDOOR 0x63
#define STOPELEVATOR 0x62
 
///////////////////////////////////////////////////////
//Floor Request Types
 
enum FloorRequest { NO_REQUEST, DISPATCHER, PASSENGER } ;
 
////////////////////////////////////////////////////////
 
// Structures
 
typedef struct elevatorStatus{
  char id;
  
  int udpPort;
  
  char doorOpen;
  char lastKnownFloor;
  char ticksToNextFloor;
  char direction;
  char curDestination;
 
  int  currentWeight;    
  
  enum FloorRequest destinations[MAX_NUM_FLOORS+1];
 
} elevatorStatus;
 
 
typedef struct elevatorMessage{
  char messageType;
  char buf[MAX_MESSAGE_SIZE-1];
} elevatorMessage;
 
 
/////////////////////////////////////////////////////////
//Globals
elevatorStatus curStatus;
int topFloor=MAX_NUM_FLOORS;
 
char GDHostName[MAX_HOSTNAME_LENGTH];
char GDPort[MAX_PORT_LENGTH];

//This keepss track of the directions each floor request
//is wanting to go.  It is not important to share this with
//The G Dispatcher, since it is the one telling the elevator anyway.
//This will be protected with the curStatusMutex
char requestDirections[MAX_NUM_FLOORS+1];
 
//For determining if it was asked to shutdown,
//or if it was manually killed
char serverShutdown;
 
int serverSocket;
int personDispatcherSocket;
 
char exiting,disconnected;
 
char personInDoor;
RT_MUTEX personInDoorMutex;
 
//The elevator threads
RT_TASK groupDispatcherCommTask;
RT_TASK personDispatcherCommTask;
RT_TASK statusUpdateTask;
RT_TASK floorRunSimTask;
RT_TASK supervisorTask;
 
//Current Status Mutex
RT_MUTEX curStatusMutex;
 
//For protecting the state of the conenction variable
RT_MUTEX connectionMutex;
char shuttleMode;
 
//For protecting the global for transmitting if the door is being held
RT_MUTEX doorHeldMutex;
char doorHeld;
 
//Condition variable to make the elevator simulator wait until requested to run
RT_MUTEX elevatorSimMutex;
RT_COND elevatorSimCond;
char simReady;
 
//Double Buffer Stuff
RT_MUTEX buf1Mutex,buf2Mutex,newestDataMutex;
char mostRecentData;
char statusBuf1[sizeof(elevatorStatus)];
char statusBuf2[sizeof(elevatorStatus)];
 

//Start time, for time stamps
time_t initialTime;

/////////////////////////////////////////////////////////
 
int packData(char *buffer, int bufIndex, char * data, int datalength)
{
    int i;
    for (i = 0;i < datalength;i++, bufIndex++)
    {
        *(buffer+bufIndex)= data[i];
    }
    return bufIndex;
}
 
void packStatusBuffer(char *buffer){
  int bufIndex = 0;
  
  rt_mutex_acquire(&curStatusMutex,TM_INFINITE);
 
  //Pack ID
  bufIndex = packData(buffer, bufIndex, &(curStatus.id), sizeof(curStatus.id));
 
  //Pack UDP Port
  bufIndex = packData(buffer, bufIndex, (char *) &(curStatus.udpPort), sizeof(curStatus.udpPort));
 
  //Pack door open
  bufIndex = packData(buffer, bufIndex, &(curStatus.doorOpen), sizeof(curStatus.doorOpen));
 
  //Pack last known floor
  bufIndex = packData(buffer, bufIndex, &(curStatus.lastKnownFloor), sizeof(curStatus.lastKnownFloor));
 
  //Pack ticks to next floor
  bufIndex = packData(buffer, bufIndex, &(curStatus.ticksToNextFloor), sizeof(curStatus.ticksToNextFloor));
 
  //Pack direction
  bufIndex = packData(buffer, bufIndex, &(curStatus.direction), sizeof(curStatus.direction));
 
  //Pack direction
  bufIndex = packData(buffer, bufIndex, &(curStatus.curDestination), sizeof(curStatus.curDestination));
 
  //Pack current weight
  bufIndex = packData(buffer, bufIndex, (char *) &(curStatus.currentWeight), sizeof(curStatus.currentWeight));
 
  //Pack list of floor requests
  bufIndex = packData(buffer, bufIndex, (char *) &(curStatus.destinations[0]), sizeof(enum FloorRequest) * MAX_NUM_FLOORS);
  
  rt_mutex_release(&curStatusMutex);
}
 
/////////////////////////////////////////////////////////
 
int isValidElevatorButton(char button){
 
  switch(button){
    case HOLDDOOR:
    case CLOSEDOOR:
    case STOPELEVATOR:
      return 1;
      break;
    default:
      return button<=topFloor;
    break;
  }
 
}
 
/////////////////////////////////////////////////////////
 
int genMessageForPDispatcher(char messageType, elevatorMessage *msg, char ackValue){
  
  msg->messageType=GENERALACK;
  memset((void *)msg->buf,0,MAX_MESSAGE_SIZE-1);
  msg->buf[0]=messageType; //Message type in 1st bit of ack
  msg->buf[1]=ackValue; //Success/Failure
  switch(messageType){
    case PERSONTRANSFER:
    case PERSONINELEVATOR:
    case PERSONPRESSBUTTON:
    case PERSONOUTELEVATOR:
      return 0;
      break;
    default:
      printf("%ld - ",time(NULL)-initialTime);
      printf("Invalid/illegal message type from ElevatorController\n");
      return -1;
      break;
  }  
}
 
int sendMessageToPDispatcher(elevatorMessage *msg, struct sockaddr_in *dest){
  int destlen = sizeof(struct sockaddr_in);
  return sendto(personDispatcherSocket,msg,sizeof(elevatorMessage),0,dest,destlen);
}
 
void parseIncomingPDispatcherMessage(elevatorMessage *msg, struct sockaddr_in *from){
  elevatorMessage newMsg;
  char inout,overload;
  int weight,curElevatorWeight;
  char buttonPressed;
 
  //printf("Msg received: %2X\n",msg->messageType);
  switch(msg->messageType){
    case PERSONTRANSFER:
      inout=(msg->buf[0]);
      weight=*((int*)((msg->buf)+1));
      overload=0;
      rt_mutex_acquire(&curStatusMutex,TM_INFINITE);
      curElevatorWeight=curStatus.currentWeight;
      rt_mutex_release(&curStatusMutex);
 
      //Only allow the passenger on if they won't overload the elevator
      if ((inout==IN) && ((curElevatorWeight+weight)>=MAX_ELEVATOR_CAPACITY)){
        genMessageForPDispatcher(PERSONTRANSFER,&newMsg,FAIL);
    overload=1;
        printf("%ld - ",time(NULL)-initialTime);
        printf("Person weighing %i too heavy for elevator\n",weight);
      }else if ((inout==OUT) &&(curElevatorWeight<weight)){
        genMessageForPDispatcher(PERSONTRANSFER,&newMsg,FAIL);
        printf("%ld - ",time(NULL)-initialTime);
        printf("Not enough weight in elevator(%i) to remove %i\n",
               curElevatorWeight,weight);
      }else{
        genMessageForPDispatcher(PERSONTRANSFER,&newMsg,SUCCESS);
      }
      sendMessageToPDispatcher(&newMsg, from);
     
      //Open the door if the person will fit
      if (!overload && inout==IN){
        //Mark that the door is currently blocked
        rt_mutex_acquire(&personInDoorMutex,TM_INFINITE);
          printf("%ld - ",time(NULL)-initialTime);
          printf("Person in door\n");
          personInDoor++;
        rt_mutex_release(&personInDoorMutex);
 
        //Add the weight
        rt_mutex_acquire(&curStatusMutex,TM_INFINITE);
          curStatus.currentWeight+=weight;
        rt_mutex_release(&curStatusMutex);
        printf("%ld - ",time(NULL)-initialTime);
        printf("Person weighing %i entering elevator\n",weight);
      }
 
      //Remove the weight from the elevator if someone is leaving
      if(inout==OUT){
        if (weight<=curElevatorWeight){
          curElevatorWeight-=weight;

          //Mark them as in the door
          rt_mutex_acquire(&personInDoorMutex,TM_INFINITE);        
            printf("%ld - ",time(NULL)-initialTime);
            printf("Person in door\n");
            personInDoor++;
          rt_mutex_release(&personInDoorMutex);

          
          printf("%ld - ",time(NULL)-initialTime);
          printf("Person weighing %i exiting elevator\n",weight);
    }else{
      printf("%ld - ",time(NULL)-initialTime);
      printf("Person leaving doesn't weigh enough!\n");
    }
 
        rt_mutex_acquire(&curStatusMutex,TM_INFINITE);
        curStatus.currentWeight=curElevatorWeight;
        rt_mutex_release(&curStatusMutex);
 
      }
 
      break;
    case PERSONINELEVATOR:
    case PERSONOUTELEVATOR:
     
      
      rt_mutex_acquire(&personInDoorMutex,TM_INFINITE);
      if (personInDoor){
        personInDoor--;
        
        printf("%ld - ",time(NULL)-initialTime);
        printf("Person out of door\n");
        genMessageForPDispatcher(msg->messageType,&newMsg,SUCCESS);
      }else{
        printf("%ld - ",time(NULL)-initialTime);
        printf("Person out of door\n");
        genMessageForPDispatcher(msg->messageType,&newMsg,FAIL);
      }
      rt_mutex_release(&personInDoorMutex);
 
      sendMessageToPDispatcher(&newMsg,from);
 
      break;
 
    case PERSONPRESSBUTTON:
      buttonPressed=msg->buf[0];
      printf("%ld - ",time(NULL)-initialTime);
      printf("Hall Call Received.  Button #%i\n",buttonPressed); 
      //Determine if the button is valid
      if (isValidElevatorButton(buttonPressed)){
        genMessageForPDispatcher(PERSONPRESSBUTTON,&newMsg,SUCCESS);
      }else{
        genMessageForPDispatcher(PERSONPRESSBUTTON,&newMsg,FAIL);
      }
      sendMessageToPDispatcher(&newMsg,from);
 
 
      //Now handle the button press
      if (buttonPressed<=topFloor){
        //Mark the hall call
    rt_mutex_acquire(&curStatusMutex,TM_INFINITE);
        curStatus.destinations[(int)buttonPressed]=PASSENGER;
    rt_mutex_release(&curStatusMutex);
 
      }else if(buttonPressed==HOLDDOOR){
        rt_mutex_acquire(&doorHeldMutex,TM_INFINITE);
        doorHeld=1;
    rt_mutex_release(&doorHeldMutex);
      }else if(buttonPressed==CLOSEDOOR){
        //Just as in most elevators, this does nothing :)
      }else if(buttonPressed==STOPELEVATOR){
        //??
      }
 
      break;
    default:
      printf("%ld - ",time(NULL)-initialTime);
      printf("Invalid/Illegal message type received: %X\n", msg->messageType);
      break;
  }
}
 
int waitForPDispatcherMessage(elevatorMessage *msg){
  struct sockaddr_in from;
  unsigned int fromlen = sizeof(struct sockaddr_in);
  int num = recvfrom(personDispatcherSocket,msg,sizeof(elevatorMessage),0,
                    &from,&fromlen);
  //printf("Read %i chars\n",num);
  parseIncomingPDispatcherMessage(msg, &from);
  return num;
}
 
 
/////////////////////////////////////////////////////////
 
void getNewestData(char *fillThis){
  char newest;
  //find buffer with newest data
  rt_mutex_acquire(&newestDataMutex,TM_INFINITE);
  newest=mostRecentData;
  rt_mutex_release(&newestDataMutex);
 
  //Copy data
  if (newest==0){
    rt_mutex_acquire(&buf1Mutex,TM_INFINITE);
    memcpy((void *)fillThis,statusBuf1,sizeof(elevatorMessage));
    rt_mutex_release(&buf1Mutex);
  }else{
    rt_mutex_acquire(&buf2Mutex,TM_INFINITE);
    memcpy((void *)fillThis,statusBuf2,sizeof(elevatorMessage));
    rt_mutex_release(&buf2Mutex);
  }
}
 
int genMessageForGDispatcher(char messageType, elevatorMessage *msg){
  msg->messageType=messageType;
  switch(messageType){
    case ELEVATORREGISTRATION:
    case ELEVATORDEREGISTRATION:
      memset((void *)msg->buf,0,MAX_MESSAGE_SIZE-1);
      rt_mutex_acquire(&curStatusMutex,TM_INFINITE);
        ((char *)msg->buf)[0]=curStatus.id;
      rt_mutex_release(&curStatusMutex);
      return 0;
      break;
    case ELEVATORSTATUS:
      memset((void *) &(msg->buf),-1,MAX_MESSAGE_SIZE);
      getNewestData((char *)&(msg->buf));
      return 0;
      break;
    default:
      printf("%ld - ",time(NULL)-initialTime);
      printf("Invalid/illegal message type from ElevatorController\n");
      return -1;
      break;
  }  
}
 
int sendMessageToGDispatcher(elevatorMessage *msg){
  return send(serverSocket,msg,sizeof(elevatorMessage),0);
}
 
void parseIncomingGDispatcherMessage(elevatorMessage *msg){
  elevatorMessage newMsg;
  char floor;
  char dir;
  //printf("Msg received: %2X\n",msg->messageType);
  switch(msg->messageType){
    case ELEVATORREGISTRATIONREPLY:
      rt_mutex_acquire(&curStatusMutex,TM_INFINITE);
      curStatus.id=((char *) msg->buf)[0];
      topFloor=((char *)msg->buf)[1]-1;
      rt_mutex_release(&curStatusMutex);
      genMessageForGDispatcher(ELEVATORSTATUS,&newMsg);
      sendMessageToGDispatcher(&newMsg);
      break;
    case ELEVATORPING:
      //printf("Ping!\n");
      genMessageForGDispatcher(ELEVATORSTATUS,&newMsg);
      sendMessageToGDispatcher(&newMsg);
      break;
    case ELEVATORDESTINATION:
      floor = ((char *)msg->buf)[0];
      rt_mutex_acquire(&curStatusMutex,TM_INFINITE);
      //Add the destination if not already been requested
      if ( curStatus.destinations[(int)floor] == NO_REQUEST){
        curStatus.destinations[(int)floor]=DISPATCHER;
        requestDirections[(int)floor]=((char *)msg->buf)[1];
      }
      
      //Mark the request direction
      if(requestDirections[(int)floor]!=((char *)msg->buf)[1]){
        if (requestDirections[(int)floor]!=BOTH && //If only one direction already, make it both
            requestDirections[(int)floor]!=IDLE){
          requestDirections[(int)floor]=BOTH;
        }else if(requestDirections[(int)floor]==IDLE){ //If idle, mark the single direction
          requestDirections[(int)floor]=((char *)msg->buf)[1];
        }
      } //Don't do anything if it is already the same.

      rt_mutex_release(&curStatusMutex);
      printf("%ld - ",time(NULL)-initialTime);
      printf("Floor %i added as Destination\n",((char *) msg->buf)[0]);
      break;
    case ELEVATORDESTINATIONREMOVE:
      floor = ((char *)msg->buf)[0];
      dir = ((char *)msg->buf)[1];
      rt_mutex_acquire(&curStatusMutex,TM_INFINITE);
        if(curStatus.destinations[(int)floor]==DISPATCHER){
          curStatus.destinations[(int)floor]=NO_REQUEST;
          if(requestDirections[(int)floor]==dir){
            requestDirections[(int)floor]=IDLE;
          }else if (requestDirections[(int)floor]==BOTH){
            requestDirections[(int)floor]=(dir==UP?DOWN:UP);
          }
          if (requestDirections[(int)floor]==IDLE){
            curStatus.destinations[(int)floor]=NO_REQUEST;
          }      
          printf("%ld - ",time(NULL)-initialTime);
          printf("Floor %i removed as Destination\n",((char *) msg->buf)[0]);
        }
      rt_mutex_release(&curStatusMutex);
      break;
    case KILLORDER:
    case ELEVATORDEREGISTRATION:
      rt_mutex_acquire(&curStatusMutex,TM_INFINITE);
      exiting=1;
      serverShutdown=1;
      rt_mutex_release(&curStatusMutex);
 
      break;
    default:
      printf("%ld - ",time(NULL)-initialTime);
      printf("Invalid/Illegal message type received: %X\n", msg->messageType);
      break;
  }
}
 
int waitForGDispatcherMessage(elevatorMessage *msg){
  int num = recv(serverSocket,msg,sizeof(elevatorMessage),0);
  //printf("Read %i chars\n",num);
  parseIncomingGDispatcherMessage(msg);
  return num;
}
 
////////////////////////////////////////////////////////
 
void updateBuffer(){
    //Check to see where the data was last stored, and now write to the other one
    int bufferToFill=0;
    rt_mutex_acquire(&newestDataMutex,TM_INFINITE);
    if (mostRecentData==0){
      bufferToFill=1;
    }else{
      bufferToFill=0;
    }
    rt_mutex_release(&newestDataMutex);
 
 
    //Pack data to double buffer
    if (bufferToFill==0){
      rt_mutex_acquire(&buf1Mutex,TM_INFINITE);
      packStatusBuffer(statusBuf1);
      rt_mutex_release(&buf1Mutex);
    }else{
      rt_mutex_acquire(&buf2Mutex,TM_INFINITE);
      packStatusBuffer(statusBuf2);
      rt_mutex_release(&buf2Mutex);
    }
 
 
    //Mark the data as being the most recent data
    rt_mutex_acquire(&newestDataMutex,TM_INFINITE);
    mostRecentData=bufferToFill;
    rt_mutex_release(&newestDataMutex);
}
 
 
void initElevator(){
  curStatus.id=0;
  curStatus.doorOpen=0;
  curStatus.lastKnownFloor=0;
  curStatus.ticksToNextFloor=TICKS_BETWEEN_FLOORS;
  curStatus.direction=IDLE;
  curStatus.curDestination=0;
  curStatus.currentWeight=0;
 
  curStatus.udpPort=0;
 
  int i;
  for (i=0;i<MAX_NUM_FLOORS;i++){
    curStatus.destinations[i]=NO_REQUEST;
    requestDirections[i]=IDLE;
  }
}
 
void reInitHallCalls(){
  //Kill all hallcalls, and bring the elevator back to the ground
  int i;
  for (i=0;i<MAX_NUM_FLOORS;i++){
    curStatus.destinations[i]=NO_REQUEST;
    requestDirections[i]=IDLE;
  }
  curStatus.destinations[0]=PASSENGER;
}
 
void activateShuttleMode(){
  int i;
  printf("%ld - ",time(NULL)-initialTime);
  printf("Keeping Elevator in Shuttle Mode\n");
  for (i=0;i<topFloor;i++){
    rt_mutex_acquire(&curStatusMutex,TM_INFINITE);
    curStatus.destinations[i]=PASSENGER;
    rt_mutex_release(&curStatusMutex);
  }
}
 
 
int connectToGDispatcher()
{
  int returnValue=0;
  int s;
  struct addrinfo hints;
  struct addrinfo *result, *rp;
 
  /* Obtain address(es) matching host/port */
  memset(&hints, 0, sizeof(struct addrinfo));
  hints.ai_family = AF_UNSPEC;    /* Allow IPv4 or IPv6 */
  hints.ai_socktype = SOCK_STREAM; /* Datagram socket */
  hints.ai_flags = 0;
 
  //This line is making things blow up in my xenomai VM...
  hints.ai_protocol = 0;          /* Any protocol */ 
  
  s = getaddrinfo(GDHostName, GDPort, &hints, &result);
  if (s != 0) {
    fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(s));
    exit(EXIT_FAILURE);
  }
 
  /* getaddrinfo() returns a list of address structures.
     Try each address until we successfully connect(2).
     If socket(2) (or connect(2)) fails, we (close the socket
     and) try the next address. */
  for (rp = result; rp != NULL; rp = rp->ai_next) {
      serverSocket = socket(rp->ai_family, rp->ai_socktype,
                   rp->ai_protocol);
      if (serverSocket == -1){
        printf("%ld - ",time(NULL)-initialTime);
        printf("continuing\n");
        continue;
      }
      if (connect(serverSocket, rp->ai_addr, rp->ai_addrlen) != -1){  
        printf("%ld - ",time(NULL)-initialTime);
        printf("Connect succeeded\n");
    returnValue=1;
        break;                  /* Success */
      }
      close(serverSocket);
  }
 
  if (rp == NULL) {               /* No address succeeded */
    fprintf(stderr, "Could not connect\n");
    returnValue=0;
  }
 
  freeaddrinfo(result);           /* No longer needed */
  
  if (returnValue==1){
    //mark as connected
    rt_mutex_acquire(&connectionMutex,TM_INFINITE);
    disconnected=0;
    rt_mutex_release(&connectionMutex);
  
    //Register to group dispatcher 
    elevatorMessage em;
    if(0==genMessageForGDispatcher(ELEVATORREGISTRATION,&em)){
      sendMessageToGDispatcher(&em);
      //printf("Sent message, %i bytes\n",n);
    }
  }
 
  return returnValue;
}

char isHallCall(){
  int i;
  char retVal=0;
  rt_mutex_acquire(&curStatusMutex,TM_INFINITE); //As per milestone 3 comments
  for (i=0;i<topFloor;i++){
    if(curStatus.destinations[i]){  //If there is a floor with a request
      retVal=1;  //There is a hall call available
    }
  }
  rt_mutex_release(&curStatusMutex);
  return retVal;
}

int requestsInDirection(char direction){
  int i;
  int num=0;
  if (direction==UP){
    for (i=curStatus.lastKnownFloor+1;i<=topFloor;i++){
      if(curStatus.destinations[i]){
        num++;
      }
    }
  }else{
    for (i=curStatus.lastKnownFloor-1;i>=0;i--){
      if(curStatus.destinations[i]){
        num++;
      }
    }
  }
  return num;
}
 
////////////////////////////////////////////////////////
//                                                    //
//                THREAD HANDLERS                     //
//                                                    //
////////////////////////////////////////////////////////
 
/////
// Communications thread for the Group Dispatcher
/////
void groupDispatcherHandler(void *arg){
  int value=0;
  int connectValue;
  char reconnected=0;
  while(!exiting){
  //Socket is closed if we get a -1
    do{
      //printf("Waiting for GD message\n");
      elevatorMessage msg; 
      memset(&msg,0, sizeof(elevatorMessage));
      value=waitForGDispatcherMessage(&msg);
    }while (value>0);
    printf("%ld - ",time(NULL)-initialTime);
    printf("Socket Closed\n");
 
    rt_mutex_acquire(&connectionMutex,TM_INFINITE);
    disconnected = 1; 
    rt_mutex_release(&connectionMutex);
    
   //Attempt to reconnect
   while(!reconnected && !exiting){
     rt_mutex_acquire(&connectionMutex,TM_INFINITE);
     if (shuttleMode){
       activateShuttleMode();
       shuttleMode=0;
     }
     rt_mutex_release(&connectionMutex);
 
     //Wait one second to attempt reconnect
     rt_task_sleep(1000*MILLISECONDS);
     connectValue=connectToGDispatcher();
 
     if (connectValue==1){
       //Success
       reconnected=1;
 
       //Send the elevator back to the first floor
       reInitHallCalls();
     }
     
   }
 
   reconnected=0;
 
 
  }
  printf("%ld - ",time(NULL)-initialTime);
  printf("Exiting...\n");
}
 
/////
// Communications thread for the Person Dispatcher
/////
void personDispatcherHandler(void *arg){
  int port=BASE_UDP_PORT;
  char portStr[PORT_STRING_LENGTH];
  struct addrinfo hints;
  struct addrinfo *result,*rp;
 
  char success = 0;
 
  hints.ai_family = AF_UNSPEC;
  hints.ai_socktype = SOCK_DGRAM;
  hints.ai_flags = AI_PASSIVE;
  hints.ai_protocol = 0;
  hints.ai_canonname = NULL;
  hints.ai_addr = NULL;
  hints.ai_next = NULL;
  
  while (!success && port<MAX_UDP_PORT){
    sprintf(portStr,"%i",port);
    getaddrinfo(NULL, portStr, &hints, &result);
  
    for (rp = result; rp !=NULL && !success;rp=rp->ai_next){
      personDispatcherSocket = socket (rp->ai_family, rp->ai_socktype,
                                       rp->ai_protocol);
      if (personDispatcherSocket == -1){
        continue;
      }
 
      if (bind(personDispatcherSocket,rp->ai_addr,rp->ai_addrlen) == 0){
        success =1; //Success
    break;
      }
 
      close (personDispatcherSocket);
    }
    if (!success){
      port++; //We'll try the next port, since this one isn't available
    }
  }
 
  if (rp == NULL) {
    fprintf(stderr, "Unable to bind to any port\n");
    return;
  }
 
  freeaddrinfo(result); //Don't need this any more
  
  rt_mutex_acquire(&curStatusMutex,TM_INFINITE);
  curStatus.udpPort=port;
  rt_mutex_release(&curStatusMutex);
 
      
  while (!exiting){
    //printf("Waiting for PD message\n");
    elevatorMessage msg; 
    memset(&msg,0, sizeof(elevatorMessage));
    waitForPDispatcherMessage(&msg);
  }
  printf("%ld - ",time(NULL)-initialTime);
  printf("Socket Closed\n"); 
}
 
 
/////
// Status Update thread.  Packs the elevator status, and passes it 
// to the communications thread.
/////
void statusUpdateHandler(void *args){
  //Set current task to be periodic every 75 milliseconds
  rt_task_set_periodic(NULL,TM_NOW,TIME_BETWEEN_UPDATES*MILLISECONDS);
 
 
  while(1){
    //Wait until the task releases at the next 75 millisecond interval
    rt_task_wait_period(NULL);
    
    rt_mutex_acquire(&curStatusMutex,TM_INFINITE);


    //If not idle, move the elevator
    if (curStatus.direction!=IDLE){ 
      //Move the elevator a bit
      curStatus.ticksToNextFloor--;
      if (curStatus.ticksToNextFloor==0){
        //Turn on the elevator simulator thread
        rt_mutex_acquire(&elevatorSimMutex,TM_INFINITE);
        simReady=1;
        rt_cond_signal(&elevatorSimCond);
        rt_mutex_release(&elevatorSimMutex);
      
      }
    }else{  //If idle, check to see if there are new hall calls
      if(isHallCall()){
        //Turn on the elevator simulator thread
        rt_mutex_acquire(&elevatorSimMutex,TM_INFINITE);
        simReady=1;
        rt_cond_signal(&elevatorSimCond);
        rt_mutex_release(&elevatorSimMutex);
      } 
    }
    rt_mutex_release(&curStatusMutex);
 
    //Update the buffer with the latest info
    updateBuffer();  
  }
}
 
 
/////
//supervisor thread
//Performs "various self-diagnostics operations at 500 ms intervals
//Also runs a shuttle-mode backup system, to circulate the elevator
//to keep things moving when the groupDispatcher is disconnected.
//This thread provides the updates normally provided by the communications
//thread when in shuttle mode.
/////
void supervisorHandler(void *args){
  rt_task_set_periodic(NULL,TM_NOW,SUPERVISOR_THREAD_PERIOD*MILLISECONDS);
  while(1){
      rt_task_wait_period(NULL);
      //Skip over this loop if we are no longer disconnected
      rt_mutex_acquire(&connectionMutex,TM_INFINITE);
      if (disconnected){ 
        shuttleMode=1;
      }else{
        shuttleMode=0;
      }
      rt_mutex_release(&connectionMutex);
 
 
  } 
}
 
/////
//floorRun thread
//Performs the actual floor-to-floor runs - stuff between a start and
//a stop.  This includes opening and closing the door, setting the car
//position indicator, and the direction arrows
//This is state driven and should only run when requested
/////
void elevatorSimulator(void *args){
  char doorNeededOpen=0;
  char requestDirection=0;
  char initialDirection=IDLE;
  char requestsAbove=0;
  char requestsBelow=0;
  
  while(1){
    
    //Wait until we are requested to run
    rt_mutex_acquire(&elevatorSimMutex,TM_INFINITE);
      while(!simReady){
        rt_cond_wait(&elevatorSimCond,&elevatorSimMutex,TM_INFINITE);
      }
      simReady=0;
    rt_mutex_release(&elevatorSimMutex);
 
    //Now do the work
 
    rt_mutex_acquire(&curStatusMutex,TM_INFINITE);
    
    //Change the floor number
    if (curStatus.direction==IDLE){
      //Do nothing
    }else if (curStatus.direction==UP){
      curStatus.lastKnownFloor=curStatus.lastKnownFloor+1;
    }else if (curStatus.direction==DOWN){
      curStatus.lastKnownFloor=curStatus.lastKnownFloor-1;
    }
 
    //Check to see if we need to stop on this floor
    doorNeededOpen = curStatus.destinations[(int)curStatus.lastKnownFloor];
    requestDirection = requestDirections[(int)curStatus.lastKnownFloor];
    initialDirection = curStatus.direction;
    //Make sure not to stop if we are going up and request is going down.
    if (doorNeededOpen == DISPATCHER && 
        requestDirection!=curStatus.direction && 
        curStatus.direction!=IDLE &&
        requestsInDirection(initialDirection)!=0){
      doorNeededOpen=NO_REQUEST;
    }
    
    printf("%ld - ",time(NULL)-initialTime);
    printf("Elevator now on floor %i\n",curStatus.lastKnownFloor);

    //Reset the ticks to next floor
    curStatus.ticksToNextFloor=TICKS_BETWEEN_FLOORS;
 
    rt_mutex_release(&curStatusMutex);
 
    rt_mutex_acquire(&curStatusMutex,TM_INFINITE);

    //Change the elevator direction if necessary
    requestsAbove=requestsInDirection(UP);
    requestsBelow=requestsInDirection(DOWN);
 
    if (initialDirection==DOWN){
      if (!requestsBelow && requestsAbove){
        curStatus.direction=UP;
      }else if (!requestsAbove && !requestsBelow){
        curStatus.direction=IDLE;
      }
    }else if(initialDirection==UP){
      if (requestsBelow && !requestsAbove){
        curStatus.direction=DOWN;
      }else if (!requestsAbove && !requestsBelow){
        curStatus.direction=IDLE;
      }
    }else{
      if(requestsBelow){
        curStatus.direction=DOWN;
      }else if(requestsAbove){
        curStatus.direction=UP;
      }else{
        curStatus.direction=IDLE;
      }
    }
    rt_mutex_release(&curStatusMutex);

    //Open the door if necessary
    if (doorNeededOpen){
      //Open the door    
      printf("%ld - ",time(NULL)-initialTime);
      printf("Elevator door open\n");
      rt_mutex_acquire(&curStatusMutex,TM_INFINITE);
      curStatus.doorOpen=1;
      rt_mutex_release(&curStatusMutex);
 
      //Leave the door open, resetting timer if door is held
      int remainingDoorOpenCount;
      for (remainingDoorOpenCount=DEFAULT_DOOR_TIME;
           remainingDoorOpenCount>0;
           remainingDoorOpenCount--){
 
    //Check to see if the door hold button has been pressed   
        rt_mutex_acquire(&doorHeldMutex,TM_INFINITE);
        if (doorHeld){
          remainingDoorOpenCount=DEFAULT_DOOR_TIME; //Restart countdown
      doorHeld=0; //Door hold acknowledged
        }
        rt_mutex_release(&doorHeldMutex);
 
        rt_task_sleep(1000*MILLISECONDS); //Wait 1 second
 
        //Check to see if there is someone in the door when it is done being open
    if (remainingDoorOpenCount==1){
      rt_mutex_acquire(&personInDoorMutex,TM_INFINITE);
      if (personInDoor){
        remainingDoorOpenCount=DEFAULT_DOOR_TIME;
      }
      rt_mutex_release(&personInDoorMutex);
    }
 
      }
    
      //Door is done being open, now close it
      rt_mutex_acquire(&curStatusMutex,TM_INFINITE);
      curStatus.doorOpen=0;
      printf("%ld - ",time(NULL)-initialTime);
      printf("Elevator door closed\n");
      
      //Reset the ticks to next floor, and remove any sim requests
      curStatus.ticksToNextFloor=TICKS_BETWEEN_FLOORS;
 
      //Mark the hall call as answered
      if (doorNeededOpen==PASSENGER){
        //If the person isn't getting on, but a passenger was getting off, make sure
        //the request isn't lost
        if (requestDirection!=IDLE && 
            requestDirection!=initialDirection && 
            requestDirection!=BOTH){
          curStatus.destinations[(int)curStatus.lastKnownFloor]=DISPATCHER;
        }else if(requestDirection!=IDLE && requestDirection==BOTH) { //If there are people going both dirs
          curStatus.destinations[(int)curStatus.lastKnownFloor]=DISPATCHER;
          requestDirections[(int)curStatus.lastKnownFloor]=(curStatus.direction==UP?DOWN:UP);
        }else{ //Person is getting on while passenger was getting off, clear it
          curStatus.destinations[(int)curStatus.lastKnownFloor]=NO_REQUEST;
          requestDirections[(int)curStatus.lastKnownFloor]=IDLE;
        }
      }else{
        curStatus.destinations[(int)curStatus.lastKnownFloor]=NO_REQUEST;
        requestDirections[(int)curStatus.lastKnownFloor]=IDLE;
      }
      rt_mutex_release(&curStatusMutex);

      //remove any sims that have been requested to run
      rt_mutex_acquire(&elevatorSimMutex, TM_INFINITE);
        simReady=0;
      rt_mutex_release(&elevatorSimMutex);
    }
 
    
 
  }//End of main while
}
 
////////////////////////////////////////////////////////
  
void cleanupThreads(){
  
  
  rt_task_delete(&personDispatcherCommTask);
  rt_task_delete(&groupDispatcherCommTask);
  rt_task_delete(&statusUpdateTask);
  rt_task_delete(&floorRunSimTask);
  rt_task_delete(&supervisorTask);
 
  //Send a deregistration message, if killed manually
  if (!serverShutdown && !disconnected){
    elevatorMessage em;
    if(0==genMessageForGDispatcher(ELEVATORDEREGISTRATION,&em)){
      int n = sendMessageToGDispatcher(&em);
      printf("%ld - ",time(NULL)-initialTime);
      printf("Sent Deregistration, %i bytes\n",n);
    }
  }
 
  close(serverSocket);
  close(personDispatcherSocket);
 
  rt_mutex_delete(&buf1Mutex);
  rt_mutex_delete(&buf2Mutex);
  rt_mutex_delete(&newestDataMutex);
  rt_mutex_delete(&curStatusMutex);
  rt_mutex_delete(&connectionMutex);
  rt_mutex_delete(&doorHeldMutex);
  rt_mutex_delete(&personInDoorMutex);
 
  rt_mutex_delete(&elevatorSimMutex);
  rt_cond_delete(&elevatorSimCond);
  
  exit(0);
 
}
 
 
 
 
int main(int argc, char *argv[]){
  
  initElevator();
 
  disconnected=1; //Everything is initially disconnected
  doorHeld=0; //Door is initially not held
  personInDoor=0; //No-one in the door initially
  serverShutdown=0; //We'll assume it was CTRL-C, unless otherwise

  //Get initial time
  initialTime = time(NULL);
 
  //Catch CTRL-C
  signal(SIGTERM,cleanupThreads);
  signal(SIGINT,cleanupThreads);
 
  if (argc < 2) {
    fprintf(stderr, "SYSC 3303 Elevator Controller - Max Joyce\n");
    fprintf(stderr, "Usage: %s host port\n", argv[0]);
    exit(EXIT_FAILURE);
  }
 
  //Store the hostname and port
  strcpy(GDHostName,argv[1]);
  strcpy(GDPort,argv[2]);
 
  connectToGDispatcher(); 
 
  //Now I'm connected, and can do whatever Elevatory stuff I want.
  
  mlockall(MCL_CURRENT | MCL_FUTURE);
  
  //Initialize the mutexes, semaphores, etc.
  rt_mutex_create(&curStatusMutex,NULL);
  rt_mutex_create(&buf1Mutex,NULL);
  rt_mutex_create(&buf2Mutex,NULL);
  rt_mutex_create(&newestDataMutex,NULL);
 
  rt_mutex_create(&connectionMutex,NULL);
 
  rt_mutex_create(&doorHeldMutex,NULL);
  rt_mutex_create(&personInDoorMutex,NULL);
 
  rt_mutex_create(&elevatorSimMutex,NULL);
  rt_cond_create(&elevatorSimCond,NULL);
  simReady=0;
 
  //Initialize the newestData
  rt_mutex_acquire(&newestDataMutex,TM_INFINITE);
  mostRecentData=NO_DATA_BUFFED;
  rt_mutex_release(&newestDataMutex);
 
  updateBuffer();
 
  exiting = 0;
  //Try to start the tasks
  if (0==rt_task_create(&groupDispatcherCommTask,NULL,0,99,T_JOINABLE)){
    rt_task_start(&groupDispatcherCommTask,&groupDispatcherHandler,NULL);
  }else{
    fprintf(stderr, "Unable to start Group Dispatch message Handler thread\n");
    return -1;
  }
  if (0==rt_task_create(&personDispatcherCommTask,NULL,0,99,T_JOINABLE)){
    rt_task_start(&personDispatcherCommTask,&personDispatcherHandler,NULL);
  }else{
    fprintf(stderr,"Unable to start Person Dispatch message handler thread\n");
    rt_task_delete(&groupDispatcherCommTask); //We need to clean up the already started thread
    return -2;
  }
 
 
  rt_task_create(&statusUpdateTask,NULL,0,99,T_JOINABLE);
  rt_task_create(&floorRunSimTask,NULL,0,99,T_JOINABLE);
  rt_task_create(&supervisorTask,NULL,0,99,T_JOINABLE);
 
  rt_task_start(&statusUpdateTask,&statusUpdateHandler,NULL);
  rt_task_start(&floorRunSimTask,&elevatorSimulator,NULL);
  rt_task_start(&supervisorTask,&supervisorHandler,NULL);
 
 
  
  //If the group dispatcher task ends, we end everything
 
  rt_task_join(&groupDispatcherCommTask); 
  
  cleanupThreads();
 
  return 0;
 
}
 
 

